clear variables; close all; clc; rng(10)


%--------------------set simulation parameter-------------------------------------------
parameters.N = 100;                                     %number of time steps
parameters.MPiter = 2;                                  %number of message passing iterations

L = 4;                                                  %sigma point parameter
kappa = 3 - L;
parameters.sigmapar.alpha = 0.01;  
parameters.sigmapar.lambda = parameters.sigmapar.alpha^2*(L + kappa) - L;                               
parameters.sigmapar.beta = 2;
          
parameters.sampling.numParticles = 1000;                %number of particles

parameters.varV = 0.01;                               %variance of driving noise
parameters.varM = 1;                                    %measurement variance
parameters.A = eye(4,4);                                %state transition matrix
parameters.A(1,3) = 1;
parameters.A(2,4) = 1;

parameters.W = zeros(4,2);                              %noise transformation matrix
parameters.W(1,1) = 0.5;
parameters.W(2,2) = 0.5;
parameters.W(3,1) = 1;
parameters.W(4,2) = 1;

topology.ac = [[0 25];[50 25]];                         %anchor positions
[S, ~] = size(topology.ac);

pNdEnd= [[10 50];[50 10];[0 20]];                       %trajectory start and endpoints of mobile agents
pNdStart = [[0 0];[25 50];[50 0]];
[M,~] = size(pNdStart);                                 % M is number of mobile agents

topology.conNdAc = ones(M,S);                           %network topology
topology.conNdNd = ones(M) - eye(M);


%--------------------generate trajectories----------------------------------------------
topology.trajec = generateTrajSelfLoc(pNdStart, parameters.varV, parameters.N+1, pNdEnd);


%--------------------specify prior distribution-----------------------------------------
initalisation.cov = zeros(4,4,M);
for  m=1:M
    initalisation.cov(:,:,m) = [[10 0 0 0];[0 10 0 0];[0 0 0.01 0];[0 0 0 0.01]];
end

initalisation.mean = zeros(4,M);
for  m=1:M
    initalisation.mean(:,m) = [topology.trajec(1,1,m)+sqrt(initalisation.cov(1,1,m))*randn;topology.trajec(2,1,m)+sqrt(initalisation.cov(2,2,m))*randn;topology.trajec(3,1,m)+sqrt(initalisation.cov(3,3,m))*randn;topology.trajec(4,1,m)+sqrt(initalisation.cov(4,4,m))*randn];
end


%--------------------generate measurements----------------------------------------------
measurements.NdAc = zeros(M,S,parameters.N);
measurements.NdNd = zeros(M,M,parameters.N);
for n = 1:parameters.N
    for i=1:M
        for j=1:S
            if( topology.conNdAc(i,j) )
                measurements.NdAc(i,j,n) = sqrt((topology.ac(j,1) - topology.trajec(1,n+1,i))^2+(topology.ac(j,2) - topology.trajec(2,n+1,i))^2) + sqrt(parameters.varM)*randn;
            end
        end
        
        for j=1:M
            if( topology.conNdNd(i,j) )
                measurements.NdNd(i,j,n) = sqrt((topology.trajec(1,n+1,j)- topology.trajec(1,n+1,i))^2+(topology.trajec(2,n+1,j) - topology.trajec(2,n+1,i))^2) + sqrt(parameters.varM)*randn;
            end
        end
    end
end


%--------------------execute belief propagations algorithms-----------------------------
runtime = zeros(3,1);
error = zeros(3,1);
tic
[estimatedTrajectory, mse] = selfLocSigmaPoints(initalisation, parameters, topology, measurements);
runtime(1) = toc;
tic
[estimatedTrajectory2, mse2] = selfLocNbp2(initalisation, parameters, topology, measurements);
runtime(2) = toc;
tic
[estimatedTrajectory3, mse3] = selfLocNbp1(initalisation, parameters, topology, measurements);
runtime(3) = toc;
error(1) = mean(mean(mse(2:end,:)));
error(2) = mean(mean(mse2(2:end,:)));
error(3) = mean(mean(mse3(2:end,:)));


%--------------------plot results--------------------------------------------------------
printAll( estimatedTrajectory, estimatedTrajectory2, estimatedTrajectory3, mse, mse2, mse3, topology )
error
runtime